{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Optimizers in SciPy\n",
    "This notebook is a very brief introduction to SciPy optimizers, documenting the example appendix/scipy_optim.py."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "There are several optimizers in SciPy, in the module scipy.optimize. You can simply install them with +pip install scipy. \n",
    "You may find the user manual of this module in https://docs.scipy.org/doc/scipy/tutorial/optimize.html#tutorial-sqlsp.\n",
    "\n",
    "In this serie of notebooks about robotics, we mostly use BFGS, a quasi-Newton constraint-free algorithm, and SLSQP, a sequential QP solver accepting both equality and inequality constraints.\n",
    "\n",
    "We will then need the two +fmin functions from the scipy.optimize module, as well as +numpy to represent algebraic vectors."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.optimize import fmin_bfgs, fmin_slsqp\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "\n",
    "They are generally following a similar API, taking as main argument the cost function to optimize +f, the initial guess +x0, and optiminally a callback function +callback and some constraints.\n",
    "\n",
    "The cost objective should be defined as a function mapping the parameter space $x$ to a real value $f(x)$. Here is a simple polynomial example for $x \\in R^2$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def cost(x):\n",
    "    '''Cost f(x,y) = x^2 + 2y^2 - 2xy - 2x '''\n",
    "    x0 = x[0]\n",
    "    x1 = x[1]\n",
    "    return -1 * (2 * x0 * x1 + 2 * x0 - x0**2 - 2 * x1**2)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The callback takes the same signature but returns nothing: it only works by side effect, for example printing something, or displaying some informations in a viewer or on a plot, or possibly storing data in a logger. Here is for example a callback written as the functor of an object, that can be used to adjust its behavior or store some data."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "class CallbackLogger:\n",
    "    def __init__(self):\n",
    "        self.nfeval = 1\n",
    "\n",
    "    def __call__(self, x):\n",
    "        print('===CBK=== {0:4d}   {1: 3.6f}   {2: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))\n",
    "        self.nfeval += 1\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For BFGS, that's all we need, as it does not accept any additional constraints. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x0 = np.array([0.0, 0.0])\n",
    "# Optimize cost without any constraints in BFGS, with traces.\n",
    "xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger())\n",
    "print('\\n *** Xopt in BFGS = %s \\n\\n\\n\\n' % str(xopt_bfgs))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In that case, the gradients of the cost are computed by BFGS using finite differencing (i.e. not very accurately, but the algorithmic cost is typically very bad). If you can provide some derivatives by yourself, it would greatly improve the result. Yet, as a first draft, it is generally not too bad."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "For SLSQP, you can simply do the same."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Optimize cost without any constraints in CLSQ\n",
    "xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1)\n",
    "print('\\n *** Xopt in LSQ = %s \\n\\n\\n\\n' % str(xopt_lsq))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, SLSQP can also handle explicit constraints. Equality and inequality constraints must be given separately as function from the parameter $x$ to a vector stacking all the numerical quantities, that must be null for equalities, and positive for inequalities.\n",
    "\n",
    "We introduce here, as an example, two set of polynomial constraints."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def constraint_eq(x):\n",
    "    ''' Constraint x^3 = y '''\n",
    "    return np.array([x[0]**3 - x[1]])\n",
    "\n",
    "def constraint_ineq(x):\n",
    "    '''Constraint x>=2, y>=2'''\n",
    "    return np.array([x[0] - 2, x[1] - 2])\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The solver then run as follows:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Optimize cost with equality and inequality constraints in CLSQ\n",
    "xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1)\n",
    "print('\\n *** Xopt in c-lsq = %s \\n\\n\\n\\n' % str(xopt_clsq))\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "That's all for now, folks."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
